#include "stdafx.h"
#include "Algeb.h"

#include "Ray.h"

using namespace rt;

static const XMVECTOR EPS_VEC = XMVectorReplicate(0.0001f);
static const float EPS = 0.00001f;

static const XMVECTOR EPS_VEC_X = XMVectorSet(0.01f, 0.0f, 0.0f, 0.0f);
static const XMVECTOR EPS_VEC_Y = XMVectorSet(0.0f, 0.01f, 0.0f, 0.0f);
static const XMVECTOR EPS_VEC_Z = XMVectorSet(0.0f, 0.0f, 0.01f, 0.0f);

const float SMALL_RAY_STEP = 0.01f;
const float BIG_RAY_STEP = 0.2f;
const float MAX_RAY_LENGTH = 50.0f;

const float ROOT_PREC = 0.03f;

const XMVECTOR SMALL_RAY_STEP_VEC = XMVectorReplicate(SMALL_RAY_STEP);
const XMVECTOR BIG_RAY_STEP_VEC = XMVectorReplicate(BIG_RAY_STEP);

float Algeb::value(const XMVECTOR & point) const
{
	// rounded box
	//XMVECTOR val = point;
	//XMVECTOR val2 = XMVectorMultiply(val, val);
	//XMVECTOR val4 = XMVectorMultiply(val2, val2);
	//XMVECTOR val6 = XMVectorMultiply(val4, val2);
	//return XMVectorGetX(val6) + XMVectorGetY(val6) + XMVectorGetZ(val6) - 8.0f;


	// rounded box
	XMVECTOR val = XMVectorSubtract(point, XMVectorSet(2.5f, 0, 0, 0));
	XMVECTOR val2 = XMVectorMultiply(val, val);
	XMVECTOR val4 = XMVectorMultiply(val2, val2);
	XMVECTOR val6 = XMVectorMultiply(val4, val2);
	float box =  XMVectorGetX(val6) + XMVectorGetY(val6) + XMVectorGetZ(val6) - 8.0f;
	XMVECTOR sval = point;
	XMVECTOR sval2 = XMVectorMultiply(sval, sval);
	float sphere = XMVectorGetX(sval2) + XMVectorGetY(sval2) + XMVectorGetZ(sval2) - 8.0f;
	//return (std::max)(box, sphere);
	return (std::max)(sphere, -box);

	
	//XMVECTOR val = XMVectorMultiply(XMVectorSubtract(point, XMVectorSet(0, 5, 0, 0)), XMVectorReplicate(0.1f));
	//XMVECTOR val2 = XMVectorMultiply(val, val);
	//float x = XMVectorGetX(val);
	//float z = XMVectorGetY(val);
	//float y = XMVectorGetZ(val);
	//float x2 = XMVectorGetX(val2);
	//float z2 = XMVectorGetY(val2);
	//float y2 = XMVectorGetZ(val2);

	//float c = z2 - 1;
	//float b = y2*y - 1;
	//float a = x2*x - 1;

	//return a*a + b*b + c*c*c;



	//XMVECTOR val = XMVectorMultiply(XMVectorSubtract(point, XMVectorSet(0, 5, 10, 0)), XMVectorReplicate(0.2f));
	//XMVECTOR val2 = XMVectorMultiply(val, val);
	//XMVECTOR val3 = XMVectorMultiply(val2, val);
	//float p = (XMVectorGetZ(val3) - 2.0f);
	//float q = XMVectorGetX(val2) + XMVectorGetY(val2) - 3;
	//return p*p + q*q*q;
}

XMVECTOR Algeb::normal(const XMVECTOR & point) const
{
	XMVECTOR diff = XMVectorSet(
		value(XMVectorAdd(point, EPS_VEC_X)) - value(XMVectorSubtract(point, EPS_VEC_X)),
		value(XMVectorAdd(point, EPS_VEC_Y)) - value(XMVectorSubtract(point, EPS_VEC_Y)),
		value(XMVectorAdd(point, EPS_VEC_Z)) - value(XMVectorSubtract(point, EPS_VEC_Z)),
		0.0f);
	return XMVector3Normalize(diff);
}

void rt::IntersectAlgebs_All_4RaysSingleOrigin(const Algeb * algebs, int num, const Ray4_SingleOrigin & rays, XMVECTOR & intersIndex, XMVECTOR & depth)
{
	for (int i = 0; i < num ; i++)
	{
		XMVECTOR newDepth = XMVectorSplatInfinity();

		for (uint rayIdx = 0; rayIdx < 4 ; rayIdx++)
		{
			XMVECTOR rayPos = rays.Orig;
			XMVECTOR rayDir = rays.GetDir(rayIdx);
			XMVECTOR smallRayStep = XMVectorMultiply(rayDir, SMALL_RAY_STEP_VEC);
			XMVECTOR bigRayStep = XMVectorMultiply(rayDir, BIG_RAY_STEP_VEC);

			for (float t = 0.0f; t < MAX_RAY_LENGTH; t += BIG_RAY_STEP) // Check ray taking big steps.
			{
				rayPos = XMVectorAdd(rayPos, bigRayStep);

				if(algebs[i].value(rayPos) < 0.0f) // If jumped into field...
				{
					//float tStart = t - BIG_RAY_STEP;
					//float tEnd = t;

					//float tPos = tEnd;

					//float oldValue = -1.0f; // only sign is important here
					//bool forward = false;



					//while(true)
					//{
					//	tPos = (tStart + tEnd) * 0.5f;
					//	float move = (tEnd - tStart) * 0.5f;//abs(oldTPos - tPos);

					//	rayPos = XMVectorMultiplyAdd(rayDir, XMVectorReplicate(tPos), rays.Orig);

					//	float newValue = algebs[i].value(rayPos);

					//	if(newValue < 0.0f && oldValue < 0.0f)
					//		forward = false;
					//	else if(newValue > 0.0f && oldValue > 0.0f)
					//		forward = true;
					//	else
					//		forward = !forward;

					//	oldValue = newValue;

					//	if(move < ROOT_PREC || abs(newValue) < 0.05f)
					//	{
					//		newDepth = XMVectorSetByIndex(newDepth, tPos, rayIdx);
					//		goto stop;
					//	}

					//	if(forward)
					//	{
					//		tStart = tPos;
					//	}
					//	else
					//	{
					//		tEnd = tPos;
					//	}
					//}


					for(float t2 = t ; t2 > t-BIG_RAY_STEP ; t2 -= SMALL_RAY_STEP) // ...start from here and take small steps back...
					{
						rayPos = XMVectorSubtract(rayPos, smallRayStep);
						if(algebs[i].value(rayPos) > 0.0f) // ...until again outside the field.
						{
							newDepth = XMVectorSetByIndex(newDepth, t2, rayIdx);
							goto stop;
						}
					}
				}
			}
stop:
			float stupidThingSoLabelCompiles;
		}


		// if newDepth < depth and ...
		UINT isNewDepthLessThanDepth;
		XMVECTOR isNewDepthLessThanDepthMask = XMVectorGreaterR(&isNewDepthLessThanDepth, depth, newDepth);
		if(XMComparisonAllFalse(isNewDepthLessThanDepth))
			continue;
		newDepth = XMVectorSelect(XMVectorZero(), newDepth, isNewDepthLessThanDepthMask); // mask out components that failed comparison,
		// so they automatically fail next one

		// ... and newDepth > EPSILON ...
		UINT isNewDepthGreaterThanEps;
		XMVECTOR isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);

		// ... set new depth(s) and intersected object indices ...
		depth = XMVectorSelect(depth, newDepth, isNewDepthGreaterThanEpsMask);
		intersIndex = XMVectorSelect(intersIndex, XMVectorSetInt(i, i, i, i), isNewDepthGreaterThanEpsMask);
	}
}

//void rt::IntersectSpheres_All_4Rays(const Sphere * spheres, int num, const Ray4 & rays, XMVECTOR & intersIndex, XMVECTOR & depth, XMVECTOR & intersType)
//{
//	for (int i = 0; i < num ; i++)
//	{
//		// B = dot(dist, rayDir)
//		XMVECTOR distX = XMVectorSubtract(rays.OrigX, XMVectorSplatX(spheres[i].CenterAndRadiusSquared));
//		XMVECTOR distY = XMVectorSubtract(rays.OrigY, XMVectorSplatY(spheres[i].CenterAndRadiusSquared));
//		XMVECTOR distZ = XMVectorSubtract(rays.OrigZ, XMVectorSplatZ(spheres[i].CenterAndRadiusSquared));
//
//		XMVECTOR mulX = XMVectorMultiply(distX, rays.DirX); // dist.x * rayDir.x
//		XMVECTOR mulY = XMVectorMultiply(distY, rays.DirY); // dist.y * rayDir.y
//		XMVECTOR B = XMVectorAdd(mulX, mulY); // dist.x * rayDir.x + dist.y * rayDir.y
//		XMVECTOR mulZ = XMVectorMultiply(distZ, rays.DirZ); // dist.z * rayDir.z
//		B = XMVectorAdd(B, mulZ); // dist.x * rayDir.x + dist.y * rayDir.y + dist.z * rayDir.z
//
//		// C = dot(dist, dist) - radius * radius
//		distX = XMVectorMultiply(distX, distX);
//		distY = XMVectorMultiply(distY, distY);
//		XMVECTOR distDotDist = XMVectorAdd(distX, distY);
//		distZ = XMVectorMultiply(distZ, distZ);
//		distDotDist = XMVectorAdd(distDotDist, distZ);
//
//		XMVECTOR C =  XMVectorSubtract(distDotDist, XMVectorSplatW(spheres[i].CenterAndRadiusSquared));
//
//		// delta = B*B - C
//		XMVECTOR delta = XMVectorSubtract(XMVectorMultiply(B, B), C);
//
//		UINT isDeltaNeg;
//		XMVECTOR isDeltaNegMask = XMVectorGreaterOrEqualR(&isDeltaNeg, XMVectorZero(), delta);
//
//		// if delta < 0.0f, move on
//		if(XMComparisonAllTrue(isDeltaNeg))
//			continue;
//
//		// root = -b - sqrtf(delta)
//		XMVECTOR root = XMVectorSubtract(XMVectorNegate(B), XMVectorSqrt(delta));
//		// set to meaningful values only components with non-negative delta
//		XMVECTOR newDepth = XMVectorSelect(root, XMVectorSplatInfinity(), isDeltaNegMask);
//		// same thing with secondary intersections (from inside the object to outside)
//		root = XMVectorAdd(XMVectorNegate(B), XMVectorSqrt(delta));
//		XMVECTOR outInters = XMVectorSelect(root, XMVectorSplatInfinity(), isDeltaNegMask);
//
//
//		//// if newDepth < depth and ...
//		//UINT isNewDepthLessThanDepth;
//		//XMVECTOR isNewDepthLessThanDepthMask = XMVectorGreaterOrEqualR(&isNewDepthLessThanDepth, depth, newDepth);
//		//if(XMComparisonAllFalse(isNewDepthLessThanDepth))
//		//	continue;
//		//newDepth = XMVectorSelect(XMVectorZero(), newDepth, isNewDepthLessThanDepthMask); // mask out components that failed comparison,
//		//// so they automatically fail next one
//
//		//// ... and newDepth > EPSILON ...
//		//UINT isNewDepthGreaterThanEps;
//		//XMVECTOR isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);
//
//		//// ... set new depth(s) and intersected object indices ...
//		//depth = XMVectorSelect(depth, newDepth, isNewDepthGreaterThanEpsMask);
//		//intersIndex = XMVectorSelect(intersIndex, XMVectorSetInt(i, i, i, i), isNewDepthGreaterThanEpsMask);
//		// 
//
//		// if newDepth > EPSILON ...
//		UINT isNewDepthGreaterThanEps;
//		XMVECTOR isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);
//
//		// take into account intersections from inside the box to the outside
//		newDepth = XMVectorSelect(outInters, newDepth, isNewDepthGreaterThanEpsMask);
//		XMVECTOR newIntersType = XMVectorSelect(XMVectorFalseInt(), XMVectorTrueInt(), isNewDepthGreaterThanEpsMask); // false value in intersType indicates that found intersection is from inside the box
//
//		isNewDepthGreaterThanEpsMask = XMVectorGreaterR(&isNewDepthGreaterThanEps, newDepth, EPS_VEC);
//		newDepth = XMVectorSelect(XMVectorSplatInfinity(), newDepth, isNewDepthGreaterThanEpsMask);
//
//		// ..  and newDepth < depth
//		UINT isNewDepthLessThanDepth;
//		const XMVECTOR isNewDepthLessThanDepthMask = XMVectorGreaterOrEqualR(&isNewDepthLessThanDepth, depth, newDepth);
//		depth = XMVectorSelect(depth, newDepth, isNewDepthLessThanDepthMask);
//		intersIndex = XMVectorSelect(intersIndex, XMVectorSetInt(i, i, i, i), isNewDepthLessThanDepthMask);
//		intersType = XMVectorSelect(intersType, newIntersType, isNewDepthLessThanDepthMask);
//
//
//
//		//// ... and new pointer(s) to intersected object
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[0] != 0.0f) intersPtr[0] = (void*)&spheres[i];
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[1] != 0.0f) intersPtr[1] = (void*)&spheres[i];
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[2] != 0.0f) intersPtr[2] = (void*)&spheres[i];
//		//if(isNewDepthGreaterThanEpsMask.m128_f32[3] != 0.0f) intersPtr[3] = (void*)&spheres[i];
//		// 
//	}
//}

void rt::GetAlgebIntersectionData(const Algeb & algeb, XMVECTOR intersPoint, IntersectionData & data)
{
	//data.Normal = XMVector3Normalize(intersPoint);
	data.Normal = algeb.normal(intersPoint);
}


//
//IntersectionInfo MetaballGroup::GetIntersectionInfo(const Vec3 & point)
//{
//	float fieldStrength = 0.0f;
//	Vec3 normalAccum = Vec3(0,0,0);
//	Material matAccum;
//	matAccum.Zero();
//
//	//for (BallIter it = _balls.begin(); it != _balls.end(); it++)
//	//{
//	//	Vec3 dist = point - it->_center;
//
//	//	float influence = it->_radius / (dist.x * dist.x + dist.y * dist.y + dist.z * dist.z);
//	//	fieldStrength += influence;
//
//	//	dist.normalize();
//	//	normalAccum += dist * influence;
//
//	//	matAccum.MultiplyAndAdd(it->_material, influence);
//	//}
//	if(fieldStrength <= 0.0f)
//		return IntersectionInfo(Vec3(0,0,0), &_material);
//
//	matAccum /= fieldStrength;
//	_material = matAccum;
//
//	return IntersectionInfo(normalAccum / fieldStrength, &_material);
//}
